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Autommous  outdoor  navigation  has  broad  ^licaticn  in  mining,  construction,  planetary  exploration,  and  military 
reconnaissance.  To  date,  most  of  the  work  tested  on  acUial  robots  has  centered  (»  local  navigation  tasks  such  as 
avoiding  obstacles  or  following  roads.  Global  navigation  has  been  limited  to  simple  waiKtering,  path  tracking, 
straight-line  goal  seeking  behaviors,  o  executing  a  sequence  of  scripted  local  behaviors.  The  problem  of  global 
navigation  in  outdoor  environments  has  been  addressed  in  the  literature,  but  it  is  generally  assumed  that  the  world 
exhibits  coarse  tt^logical  structure,  most  of  which  is  known,  and  that  sensors  and  positioi  estimation  systems 
provide  bighly-accurate  data.  These  assumptions  break  down  fo  real  robots  in  highly  unstructured  and  unknown 
environments.  With  every  image,  the  sensors  provide  r^w  information  about  the  worid  that  can  impact  the  robot’s 
path  to  the  goal.  Smne  of  the  Mormation  is  real,  some  arises  firom  noise,  and  some  arises  from  aliasing  due  to  robot 
position  errm.  Replanning  may  be  needed  for  every  image,  and  it  may  be  nontrivial  due  to  the  unstructured  nature  of 
the  environmoit  To  address  these  problems,  we  have  developed  a  complete  system  that  integrates  local  and  global 
navigation.  This  system  is  capable  of  finding  a  goal  given  no  a  prkrri  map  of  the  environment  It  is  robust  to  noise, 
vehicle  position  error,  and  is  r^le  to  rqtlan  in  real-time.  W:  describe  the  system  and  present  the  results  of  experiments 
performed  using  areal  robot 
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1.0  Introduction 

Aunonomous  outdoor  navigators  have  a  number  of  uses,  including  planetary  exidoration,  construction  site  wcnk,  min¬ 
ing,  military  reconnaissance,  and  hazardous  waste  remediation.  Tb^  tasks  require  a  mobile  robot  to  move  between 
points  in  its  environment  in  order  to  tranqxxt  cargo,  deploy  sensors,  or  rendezvous  with  other  vehicles  or  equipment 
The  problem  is  c(mq>licated  in  oivironments  that  are  unstructured  and  unknown.  In  such  cases,  the  robot  can  be 
equipped  with  one  or  more  seasms  to  measure  the  structure  of  its  environment  locate  itsdf  within  the  environment 
arid  check  to  hazards.  By  sweeping  the  terrain  to  obstacles,  recording  its  progress  through  the  environmoit  and 
building  a  map  of  sensed  areas,  the  navigator  can  find  the  goal  position  if  a  path  exists,  even  if  it  Ims  no  prior  knowl- 
e(^  of  the  environment 

A  number  of  systems  have  been  develq)ed  to  address  this  scenario  in  part.  We  limit  our  discussion  to  those  systems 
tested  on  a  real  robot  in  outdoor  terrain.  Outdoor  robots  (^)erating  in  expansive,  unstructured  envirmiments  pose  new 
problems,  since  the  c^tinud  global  routes  can  be  complicated  enough  that  simple  replarming  sqiivoaches  are  not 
feasiUe  for  real-time  (^leration.  The  bulk  of  the  work  in  outdocu'  navigation  has  centered  on  local  navigation,  that  is, 
avoiding  obstacles  [1][2][3][7][14][17][20][27][28][31]  or  foUowing  roads  [4][S][8][1S][2S].  The  global  navigation 
c^)abilities  of  these  systems  have  been  limited  to  tracking  a  pre-planned  trajectory,  wandering,  maintaining  a 
constant  beading,  ot  following  a  specific  type  of  terrain  feature.  Other  systems  with  global  navigation  components 
typically  (q)erate  by  planning  a  glolud  route  based  (m  initial  map  mfonnation  and  then  replanning  as  needed  when  the 
sensors  disraver  ah  obstructkHi  [9][10][211. 

These  ^)proaches  are  insufficient  if  there  is  no  initial  map  of  the  environment  or  if  the  environment  does  not  exhibit 
coarse,  global  structure  (e.g.,  a  anall  netwodc  of  routes  or  channels).  It  is  possible  to  replan  a  new  global  tr^ectmy 
finnn  scratch  to  each  new  piece  of  sensor  data,  but  in  cluttered  environments  the  sensors  can  detect  new  information 
almost  continuously,  thus  precluding  real-time  (qieratimi.  Furthermore,  sensm'  noise  and  vehicle  position  error  can 
lead  to  phantom  obstacle  detection  and  obstacle  aliasing  respectively,  flooding  the  global  navigator  with  more,  and 
sometimes  erroneous,  data. 

We  have  developed  a  oxnplete  navigation  system  that  solves  these  problems.  The  system  is  capable  of  driving  an 
outdoor  mobile  robot  from  an  initial  position  to  a  goal  position.  The  mobile  robot  is  equipped  with  a  range  sensw  for 
detecting  obstacles  and  a  position  estimation  system  for  determining  the  robot’s  location  in  the  world.  It  may  have  a 
prior  map  of  the  environment  or  no  map  at  alL  The  robot  is  the  Navigational  Laboratory  II  (NAVLAB  II)  shown  in 
Hgure  1.  The  NAVLAB  n  is  a  Highly  Mobile  Multi-Wheeled  Vehicle  (HMMWV)  modified  to  anuputer  control  of 
the  steering  function.  The  NAVLAB  n  is  equpped  with  an  Environmental  Institute  of  Michigan  (^IM)  scarming 
laser  rangefinder  for  measuring  the  sbrpe  of  tte  terrain  in  frcmt  of  the  vehicle.  Three  on-board  Sun  Microsystems 
SPARC  n  OHnputers  process  sensOT  data,  plan  (flrstacle  avddance  maneuvers,  and  calculate  global  paths  to  the  goal. 

Figure  2  shows  the  results  of  an  actual  run  on  die  vehicle.  The  dimensions  of  the  area  are  500  x  500  metos.  The  robot 
began  at  tte  position  labelled  S  and  moved  to  the  goal  location  at  G.  Initially,  the  robot  assumed  the  world  to  be 
devoid  of  obstacles  and  moved  toward  the  goal.  Twice  a  second,  the  perception  system  reported  the  locations  of 
obstacles  detected  by  the  sensor.  Each  time,  the  vehicle  steered  to  miss  the  obstacles  and  replanned  an  optimal  global 
trajectcuy  to  the  goal.  The  vdiicle  was  able  to  drive  at  approximately  2  meters  per  second.  The  vehicle’s  trajectory  is 
shown  in  blade,  the  detected  obstacles  in  dark  grey,  and  a  high-cost  buffer  around  the  obstacles  in  light  grey. 
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Figure  1:  NAVLAB II  Vehicle  Testbed 


Hiis  paper  describes  the  software  system  for  goal  acquisition  in  unknown  environments.  Brst,  the  global  navigator  is 
describ^  followed  by  the  local  obstacle  avoider  and  the  steering  integration  system.  Second,  experimental  results 
from  actual  vehicle  runs  are  described.  Rnally,  conclusions  and  future  work  are  described. 
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Figure  2:  Example  of  an  Actual  Run 


2.0  The  Navigation  System 

2.1  System  Overview 

Figure  3  shows  a  high-level  description  of  the  navigation  system,  which  consists  of  the  global  navigator  called  D*. 
local  navigatcH'  called  SMARTY,  and  steering  arbiter  called  the  Distributed  Archilecture  fw  Mobile  Navigation 
(DAMN).  The  global  navigator  maiiuams  a  coarse-resoiution  vaap  of  the  envirmunent,  consisting  of  a  Cartesian  lat¬ 
tice  of  grid  cells.  Each  grid  cell  is  labelled  untraversable,  high-cost,  or  traversable,  dq)aiding  on  whether  the  ceQ  is 
known  to  contain  at  least  one  obstacle,  is  near  to  an  obstacle,  or  is  fiee  of  obstacles  respectively.  For  purposes  of  our 
tests,  aU  cells  in  the  map  were  initialized  to  traversable.  The  global  navigator  initially  plans  a  trajecttny  to  the  goal 
and  sends  steering  recommendations  to  the  steering  arbiter  to  move  the  vehicle  toward  die  goal.  As  it  drives,  the  local 
navigator  sweqis  the  terrain  in  firont  of  the  vehicle  fcv  obstacles.  The  ERIM  laser  rangefinder  is  used  to  measure  the 
sluqie  of  the  terrain,  and  the  local  navigator  analyzes  the  terrain  m^s  to  find  sloped  patches  and  range  discontinuities 
likely  to  cmiesprHid  to  obstacles.  The  local  navigator  sends  steering  recommendations  to  the  arbiter  to  move  the  vehi¬ 
cle  around  these  detected  obstacles.  AcUidonally,  the  local  navigator  sends  detected  untraversable  and  traversable 
cells  to  dre  global  navigator  for  inocessing.  The  global  navigator  compares  these  cells  against  its  m^,  and  if  a  dis¬ 
crepancy  exists  (i.e.,  a  traversable  cell  is  now  untraversable  or  vice  versa),  it  plans  a  new  and  optimal  dajectory  to  the 
gori.  Tire  key  advantage  of  the  global  navigator  is  drat  it  can  effidenUy  plan  opdrrud  global  paths  and  is  able  to  gsa- 
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eiate  a  new  path  for  every  batch  of  cells  in  a  fraction  of  a  second.  The  glotel  navigator  updates  its  map  and  soids  new 
steering  recommaidali(»s  to  the  steering  arbiter. 

The  steering  recommendations  sent  to  the  arbiter  frmn  the  two  navigators  consist  of  evaluations  for  each  of  a  fixed  set 
of  constant-curvature  arcs  (i.e..  corresponding  to  a  set  of  fixed  steering  angles).  The  global  navigatw  rates  steering 
directions  highly  that  drive  toward  the  goal,  and  the  local  navigator  rates  directions  highly  that  avoid  obstacles.  The 
arbiter  combines  these  reccMtunendations  to  produce  a  single  steoing  direction  which  is  sort  to  the  vehicle  controller. 

The  rest  of  this  section  details  the  global  navigator  (D*),  the  local  navigator  (SMARTY),  the  steering  arbiter 
(DAMN),  and  the  interfaces  between  them. 

Figure  3;  Navigation  System  Diagram 


2.2  The  D*  Algorithm  for  Optimal  Replanning 
2JL1  Overview 

If  a  robot  is  equipped  with  a  corrqtlete  truq)  of  its  environment,  it  is  able  to  plan  its  entire  route  to  the  goal  before  it 
begins  moving.  A  vast  amount  of  literature  has  addressed  the  path-finding  problem  in  known  environmoits  (see 
Latombe  [18]  for  a  good  survey).  In  many  cases,  however,  this  scenario  is  unrealistic.  Often  the  robot  has  only  a  par¬ 
tial  map  or  no  mtq)  at  all  In  these  cases,  the  robot  uses  its  sensors  to  discover  the  environment  as  it  moves  and  modi¬ 
fies  its  plans  accordingly.  One  qrproach  to  path  planning  in  this  scenario  is  to  generate  a  “globaF*  path  using  the 
known  information  and  dien  circumvent  obstacles  on  the  main  route  detected  by  the  senstHS  [10],  generating  a  new 
global  plan  if  the  route  is  totally  obstructed.  Another  aiqnoach  is  to  move  directly  toward  the  goal,  skirting  the  perim¬ 
eter  of  any  obstructions  until  the  point  on  the  obstacle  neatest  the  goal  is  found,  and  then  to  proceed  directly  toward 
the  goal  again  [19].  A  third  approach  is  to  direct  the  robot  to  wander  around  the  environment  until  it  finds  the  goal, 
penalizing  forays  onto  torain  previously  traversed,  so  that  new  areas  ate  explored  [24].  A  fourth  rqrptoach  is  to  use 
m^  information  to  estimate  the  cost  to  the  goal  for  each  location  in  the  environmait  and  efficiently  update  these 
costs  with  backtracking  costs  as  the  robot  moves  through  the  environment  [16]. 

While  these  ajqxoaches  are  complete,  they  are  also  suboptimal  in  the  sense  that  they  do  not  generate  the  lowest  cost 
path,  given  the  sensor  information  as  it  is  acquired  and  assuming  all  known  a  priori  information  is  correct.  It  is 
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possiUe  to  generalB  optimal  behavior  by  using  A*  [22]  to  OMnpute  an  optimal  path  frtm  the  known  m^  information, 
moving  the  robot  along  the  path  until  either  it  reaches  tire  god  cn-  its  senstxs  detect  a  discrqiancy  bm^een  the  map 
and  the  environment,  updating  the  map,  and  thm  rq>lanning  a  new  optimal  path  CnMn  the  robot’s  current  location  to 
the  goal.  Although  this  brute-force,  replanning  approach  is  optimal,  it  can  be  grossly  inefficient,  particulariy  in 
ejqMnsive  environments  where  the  goal  is  far  away  and  little  map  information  exists. 

Tire  D*  algtvithm  (or  Dynamic  A*)  is  functionally  equivalent  u>  the  brute-force  rq>lanner  (i.e.,  sound,  complete,  and 
optimal),  but  it  is  far  more  effidoit.  For  large  «iviroiunents  requiring  a  million  map  cells  to  represent,  experimental 
results  indicate  that  it  is  over  200  times  faster  than  A*  in  replaniting,  thus  enabling  real-time  c^reration.  See  Stentz 
[29]  fcv  a  detailed  description  erf  the  algorithm  and  the  experirrrental  results. 

D*  uses  a  Cartesian  grid  of  eight-connected  cells  to  rqiresent  the  m^.  The  connections,  or  arcs,  se  labelled  with 
positive  scalar  values  indicating  the  cost  of  moving  between  the  two  cells.  Each  cell  (also  called  a  “state”)  includes  an 
estimate  of  the  path  cost  to  the  goal,  and  a  baclqreinter  to  one  of  its  neighbors  indicating  tire  direction  to  the  goal. 

Like  A*,  D*  maintains  an  OPEN  list  of  states  to  expand.  Initially,  the  goal  state  is  placed  oa  the  OPEN  list  with  an 
initial  cost  of  zero.  The  state  with  the  minimum  path  cost  on  the  OPEN  list  is  rqreatedly  expanded,  prt^jagating  path 
cost  calculations  to  its  neighbors,  until  the  optimal  cost  is  computed  to  all  cells  in  tire  nt^.  The  vehicle  then  begins  to 
move,  following  the  backpointers  toward  the  goal.  While  driving,  the  vehicle  scans  the  terrain  with  its  sensor.  If  it 
detects  an  obstacle  where  one  is  not  expected,  then  all  optirttal  paths  containing  this  arc  are  no  longer  valid.  D* 
updates  the  arc  cost  with  a  prohibitively  large  value  dettoting  an  oiKtacle,  places  the  adyoining  state  on  the  OPEN  list, 
then  repeatedly  expands  states  on  the  OPEN  list  to  propagate  the  path  cost  increase  altmg  the  invalid  paths.  The 
OPEN  list  states  tiiat  transmit  the  cost  increase  are  called  RAISE  states.  As  tire  RAISE  states  fan  out,  they  ctmre  in 
contact  with  neighbor  states  that  are  able  to  lower  their  path  costs.  These  LOW^  states  are  placed  on  the  OPEN  list 
Through  repeated  state  expansion,  the  LOWER  states  reduce  path  costs  and  redirect  backpointers  to  ctnnpute  new 
optimal  patte  to  tire  invalidated  states. 

Conversely,  if  the  vehicle’s  sensor  detects  the  absence  of  an  obstacle  where  one  is  expected,  then  a  new  optimal  path 
may  exist  from  the  vehicle  to  the  goal  (i.e.,  through  the  “missing”  obstacle).  D*  u^tes  the  arc  cost  with  a  small 
value  denoting  empty  space,  places  the  adjoining  state  on  the  OPEN  list  as  a  LOWER  state,  then  rqreatedly  expands 
states  to  compute  new  optimal  paths  wherever  possible.  In  either  case,  D*  determines  how  far  the  cost  propagation 
must  proceed  until  a  new  optimal  path  is  computed  to  the  vehicle  or  it  is  decided  that  the  old  one  is  still  (q>timal.  Once 
this  determination  has  been  made,  the  vehicle  is  fiee  to  continue  moving  (q)timally  to  the  goal,  scaiming  the  terrain 
for  obstacles. 

Hgure  4  and  Figure  5  illustrate  this  process  in  simulatioa  Figure  4  shows  a  SO  x  50  cell  environment  after  the  initial 
cost  calculation  to  all  cells  in  the  space.  The  optimal  patii  to  any  cell  can  be  determined  by  tracing  die  backpointers  to 
tire  goal.  The  light  grey  obstacle  represents  a  known  obstacle  (Le.,  one  that  is  stmed  in  the  mq>),  while  the  datic  grey 
obstacle  represents  an  unknown  obstacle.  Note  that  the  backpointers  pass  through  the  dark  grey  obstacle  since  it  is 
unknown  The  robot,  equipped  with  a  S-cell  radial  field-of-view  sensor,  starts  at  the  center  of  the  left  wall  and 
proceeds  toward  the  goal.  Initially,  it  deflects  around  tire  known  obstacle,  but  beads  toward  the  unknown  obstacle.  As 
its  sensor  detects  the  obstacle,  the  cost  increases  fan  out  from  the  obstacle  via  RAISE  states  until  they  reach  the 
LOWER  states  around  the  bottom  of  the  light  grey  obstacle.  These  LOWER  slates  redirect  the  badqiointa^  to  guide 
the  vehicle  around  the  bottom  and  to  the  goal.  Figure  5  illustrates  the  final  m^  configuration,  aftor  the  vehicle  has 
reached  the  goal  Note  that  optimal  paths  have  been  computed  to  some  but  not  all  of  the  cells  in  the  environment.  This 
effect  illustrates  the  efficiency  of  D*.  It  limits  the  cost  propagations  to  the  vicinity  of  the  obstruction,  while  still 
ensuring  that  tiie  robot’s  path  is  optimal. 

Figure  6  shows  a  450  x  450  cell  simulated  envirorunent  cluttered  with  grey  obstacles.  The  black  curve  shows  the 
optimal  patii  to  the  goaL  a.«Muining  all  of  tiie  obstacles  are  known  a  priori,  before  the  vehicle  begins  its  traverse.  This 
path  is  known  as  omniscient  optimal.  Hgure  7  shows  planning  in  tire  same  environmoit  where  none  of  the  obstacles 
are  stored  in  the  map  a  priori.  This  m^  is  known  as  optimistic,  since  the  vehicle  assumes  no  obstacles  exist  unless 
they  are  detected  by  its  15-cell  radial  sensor.  This  trajectory  is  nearly  two  times  longer  than  omniscient  optimal; 
however,  it  is  still  optimal  given  the  initial  map  and  the  sensor  information  as  it  was  acquired. 
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Figore  4:  Baciqiointars  after  Initial  Propagation 


Figure  5:  Final  BaclgM>inter  Configuration 
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Figwc  6:  Planning  witti  a  Conplete  Map 
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2^.2  Cell  Expansion  from  SMARTY  Data 

A  number  of  modifications  were  made  to  D*  to  ad^t  it  to  an  actual  robot  system.  The  system  diagram  is  shown  in 
Bgure  8.  As  the  vehicle  drives  toward  the  goal,  its  laser  rangefinder  scans  the  terrain  in  front  of  the  vehicle.  The 
SMARTY  local  navigator  processes  this  sensor  data  to  find  obstacles  and  sends  the  (x,y)  locations  of  detected  obsta¬ 
cles  (untraversaUe  cells)  to  O*  at  regular  intervals,  using  the  TCX  [6]  message  passing  syston.  Additionally, 
SMARTY  sends  (jc,y)  locations  of  cells  known  to  be  devoid  of  obstacles  (traversable  cells).  Since  the  D*  m^  is  used 
to  represent  a  global  area,  its  grid  cells  are  of  coarser  resolution  than  SMARTY’s  (i.e.,  1  meter  versus  0.4  m&a).  D"* 
keeps  track  of  the  locations  of  obstacles  within  each  of  its  grid  cells,  adding  or  deleting  obstacles  as  needed  based  on 
the  data  from  SMARTY.  If  an  obstacle  is  added  to  a  previously  empty  mrq;)  cell  or  all  of  the  obstacles  ate  deleted  fiom 
a  previously  obstructed  cell,  then  this  constitutes  a  significant  event  within  D*  since  a  traversable  cell  becomes  an 
untraversable  cell  or  an  untraversable  cell  becomes  a  traversable  cell  respectively. 


Figure  8:  D*  Overview 


Traversable/ 
untraversable  cells 


Steering  votes 
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Since  does  not  explicitly  model  the  shape  of  the  robot  (i.e.,  it  is  assumed  to  be  a  point),  the  new  untraversable 
cells  are  expanded  by  half  the  width  of  the  robot  to  {^proximate  a  configuration  sp^  (C-space)  obstacle.  All  ms^ 
cells  within  a  2-meter  radius  are  classified  as  untraversable.  Note  that  the  longer  dimension  (i.e.,  length)  of  the  robot 
is  not  used  in  the  expansion.  The  length  must  be  modelled  to  detect  possible  bumper  collisions  when  driving  forward 
or  backward;  however,  D'''  is  not  intended  to  perform  local  obstacle  avoidance.  Instead,  it  is  concerned  with  detecting 
channels  between  obstacles  that  are  wide  enough  to  admit  the  robot  if  it  can  get  properly  oriented.  While  this  heuristic 
breaks  down  in  very  cluttered  environments,  for  most  envirotunents  it  works  quite  well  and  eliminates  the  need  for  a 
three-dimensional  C-space. 

The  C-space  expansion  provides  some  buffeting  to  keep  the  robot  away  from  obstacles.  We  found  that  additional 
buffeting  in  the  form  of  a  high-cost  field  leads  to  better  performance.  The  idea  is  to  penalize  the  robot  cost-wise  fw 
passing  too  close  to  an  obstacle,  causing  the  robot  to  approach  obstacles  only  when  open  space  is  unavailable.  For 
example,  an  area  cluttered  with  trees  may  be  navigable,  but  a  route  around  the  trees  would  be  less  risky  and  therefcae 
preferable.  For  each  true  untraversable  cell  (i.e.,  those  containing  obstacles  from  SMARTY,  not  those  created  from 
the  C-space  expansion),  all  traversable  cells  within  a  radius  of  8  meters  are  classified  as  high-cost.  I  he  cost  of 
traversing  a  high-cost  cell  is  five  times  that  of  a  traversable  cell.  Therefore,  if  a  channel  between  two  obstacles 
requites  the  robot  to  drive  through  10  high-cost  cells,  it  would  choose  a  longer,  alternate  route  passing  through  up  to 
SO  traversable  cells. 
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When  an  untraversable  cell  is  changed  to  traversable,  all  of  the  corresponding  untraversable  and  high-cost  cells 
created  during  the  C-space  expansion  are  changed  to  traversable.  Every  time  a  ceU  changes  classification  (i.e.,  among 
traversable,  untravers^le,  and  high-cost),  it  is  placed  on  die  OPEN  list  for  future  consideration.  The  cost  changes  are 
pnqjagated  as  needed  to  [Hoduce  steering  recommendations  for  the  DAMN  arbiter. 

2,2.3  Steering  Arc  Evaluation  for  DAMN 

Every  500  msec,  D*  sends  steering  recommendations  to  DAMN.  D’*'  first  checks  the  vehicle  position  and  then  com¬ 
putes  the  ou^ints  of  a  static  set  of  SI  steering  arcs,  linearly  distributed  in  arc  curvahite  from  -0.125  metef  ‘  to 
+0.125  meter'*.  The  points  are  computed  to  be  at  constant  distance L  from  the  current  vehicle  position  along  the  arcs. 
L  is  curroitly  fixed  at  10  meters  for  a  speed  of  about  2  tn/sec.  D*  amverts  the  points  to  idisolute  locations  in  its  inter¬ 
nal  m^.  It  then  expands  states  on  its  OPEN  list  until  ithas  con^iuted  an  optinial  path  to  every  point  in  the  list  The 
cost  of  the  optitnal  path  from  each  point  to  the  goal  is  converted  to  a  vote  between  -1  and  +1,  where  +1  is  a  strraig  rec¬ 
ommendation  and  -1  is  a  veto,  and  is  sat  to  the  arbiter.  If  a  point  happens  to  fall  in  an  untraversable  cell,  a  cell 
unreachable  firom  the  goal,  or  a  cell  not  in  D^’s  map,  it  is  assigned  a  vote  of  -1.  Otherwise,  if  Cn^T  and  are  the 

minimum  and  maximum  values  of  the  costs  in  the  current  list  of  points,  the  vote  v  for  an  arc  is  derived  from  the  cost 
c  of  the  corresponding  point  in  the  following  way: 

This  simple  formula  ensures  that  arcs  going  through  obstacles  are  inhibited  and  that  the  preferred  direction  goes 
through  the  point  of  minimum  cost.  The  vote  for  c„„  is  set  to  0  instead  of  - 1  because  a  high  cost  means  that  the  arc  is 
less  desirable  but  should  not  be  inhibited. 

Because  D’*  does  generate  enough  information  to  inhibits  arcs  going  through  obstacles,  one  could  be  tempted  to 
eliminate  the  obstacle  avoidance  behavior  from  SMARTY  and  to  use  D*  as  the  sole  source  of  driving  commands. 
Although  it  is  quite  possible  to  configure  the  system  to  run  in  this  mode,  the  result  would  be  poor  perfcnmance  for  at 
least  three  reasons.  First,  D^’s  nup  is  lower  resolution  than  the  map  used  internally  by  SMARTY  (1  meter  vs.  0.4 
meter  in  the  cioient  implementation).  As  a  result,  D*  cannot  control  fine  motion  of  the  vehicle.  Second,  SMARTY 
typically  generates  commands  faster  than  D*  can  update  its  m^  and  propagate  costs,  thus  ensuring  lower  latency. 
Third,  SMARTY  evaluates  the  distances  between  the  arcs  and  all  the  obstacle  cells  in  the  m^  whereas  D*  evaluates 
only  the  cost  of  a  single  point  on  the  arc.  In  addition  to  these  practical  considerations,  it  is  gaerally  ill-advised  to  mix 
local  reflexive  behavior  such  as  obstacle  avoidance  and  globd  behaviors  sudi  as  path  plaiming  in  a  single  module. 

It  should  be  noted  that  the  steering  recommendatimis  produced  by  D*  lead  to  relatively  smooth  trajectories,  even 
though  the  optimal  trajectories  can  be  very  discontinuous.  D*  assumes  the  robot  is  omnidirectional  and  does  not 
penalize  abrupt  heading  changes.  The  test  vehicle,  the  NAVLAB  n,  is  car-like  and  has  a  minimum  turning  radius. 
Whereas  in  cluttered  environments  these  kinematic  constraints  must  be  modelled,  for  fairly  open  environments,  the 
omnidirectional  assumption  works.  Since  the  vehicle  is  unable  to  “turn  on  a  dime”,  it  chooses  the  tightest  steering  arc 
in  the  desired  direction  of  travel,  thus  “smoothing”  the  global  tr^ectory. 

Hnally,  we  note  that  the  interface  between  D*  and  DAMN  is  actually  implemented  as  a  sqiarate  module  connected 
throu^  TCX.  The  module  gaerates  steering  requests  to  D*  every  500  msec  and  converts  costs  to  votes  and  passes 
them  to  DAMN.  We  chose  the  distributed  tqtproach  because  it  isolates  D*  cleanly  from  the  details  of  the  driving 
system  and  because  the  additional  computation  and  scheduling  time  would  slow  down  D*’s  main  expansion  loop. 
Because  the  query  points  sent  to  D'*‘  are  expressed  with  respect  to  the  vehicle,  they  do  not  change  once  L  is  fixed.  We 
use  this  property  to  reduce  the  message  traffic  by  first  sending  m  initialization  message  to  D*  which  contains  the  list 
of  all  the  points  with  respect  to  the  vehicle.  After  initializatiai,  the  intoface  simply  sends  the  currat  vehicle  position 
to  D*  which  converts  the  points  to  its  coordinate  system  according  to  this  position.  In  this  approach,  a  request  from 
the  interface  module  consists  of  a  short  message  of  three  numbers:  x,  y,  and  beading  of  the  vehicle. 
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2.3  TtM  SMARTY  System  for  Local  Navigation 

2.3.1  Overview 

SMARTY  is  a  system  fm  controlling  a  vehicle  based  on  input  &om  a  range  senscK  Figure  9  shows  SMARTY’s  basic 
(Mganization.  The  range  data  processing  ctHnpanent  takes  a  stream  of  range  pixels  as  input  and  ouQHits  untraversable 
locations  with  respect  to  the  vehicle  positkm  at  the  time  the  data  was  acquir^.  The  local  map  manager  receives  the 
lists  of  untraversable  cells  as  soon  as  they  are  detected  by  the  range  processing  section  and  maimains  their  locatirai 
with  respect  to  current  vehicle  position.  The  local  map  manager  sends  the  entire  list  of  untraversable  cells  to  the  steer¬ 
ing  arc  evaluatitm  module  at  regular  intervals.  The  arc  evaluator  omqNites  safety  saves  fcv  a  fixed  set  of  arcs  based 
on  the  relative  positions  of  untraversable  cells  with  respect  to  the  current  vehicle  position  and  sends  them  to  the 
DAMN  steering  arbiter.  These  three  parts  are  inqilemented  as  a  single  Unix  process  in  which  the  range  data  is  pro¬ 
cessed  continuously  as  fast  as  it  as  acquired  and  the  arc  evaluate  is  activated  at  regular  time  intmvals.  Like  D*, 
SMARTY  communicates  with  external  modules  through  the  TCX  messaging  system.  We  briefly  describe  the  three 
SMARTY  cortqxrnoits  in  the  next  paragrtqrhs  and  conclude  this  section  with  a  detailed  description  (rf  the  interface 
between  D*  and  SMARTY. 


Figure  9:  SMARTY  Overview 


2.3.2  Range  Data  Proceeelng 

The  range  image  processing  module  takes  a  single  image  as  input  and  ouqxits  a  list  of  regions  which  are  untravers¬ 
able  (see  Figure  10).  The  initial  stage  of  image  filtering  resolves  the  ambiguity  due  to  the  maximum  range  of  the  scan¬ 
ner,  and  ronoves  outliers  due  to  effects  such  as  mixed  pixels  and  reflections  from  specular  surfaces.  (See  Hebert  [12] 
for  a  complete  descr^tion  of  these  effects.)  After  image  filtering,  the  (x,y,z)  location  of  every  pixel  in  the  range  image 
is  computed  in  a  coordinate  system  relative  to  the  current  vehicle  position.  The  coordinate  system  is  d^ed  so  that 
the  z  axis  is  vertical  with  respect  to  the  ground  plane.  The  transftHmation  lakes  into  account  the  orientation  of  the 
vehicle  read  from  an  inertial  navigation  system.  The  points  are  that  nuqrped  into  a  discrete  grid  on  the  (x,y)  plane. 
Each  cell  of  the  grid  contains  the  list  of  the  (x,y,z)  coordinates  of  the  points  which  fall  within  the  bounds  of  the  cell  in 
X  and  y.  The  size  of  a  cell  in  the  current  system  is  20  cm  in  both  x  and  y.  The  terrain  classification  as  traversable  or 
untraversable  is  first  perfivmed  in  every  cell  individually.  The  criteria  used  for  the  classification  are: 

>  the  height  variaticm  of  the  terrain  within  the  cell, 

•  the  orioitation  of  the  vector  normal  to  die  patch  of  terrain  contained  in  the  cell. 
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•  and  tbe  presence  of  a  discontinuity  of  elevation  in  the  ceU. 

To  avoid  frequent  erroneous  classification,  tbe  first  two  criteria  are  evaluated  only  if  the  number  of  points  in  the  cell  is 
large  enough.  In  practice,  a  mmimiun  of  five  pdnts  per  cell  is  used. 

The  processing  is  performed  scanline  by  scanline  instead  of  processing  the  entire  range  image  and  sending  the 
appropriate  map  cells  at  completion,  as  described  in  Hebert  [11].  In  scanline-  ot  pixel-based  processing,  each  pixel  is 
converted  to  Cartesian  comxlinates  and  tbe  state  of  the  oOTesponding  cell  in  the  m^  is  updated.  A  cell  is  rqrarted  as 
untraversable  as  soon  as  a  ctmqmnent  of  its  state  exceeds  some  threshold,  e.g.,  tbe  sltqie  becmnes  too  high,  and  tbe 
number  of  points  in  the  cell  becomes  high  enough  to  have  confidence  in  tbe  status  of  tbe  cell.  This  aiq)roach  has 
several  advantages  over  the  traditional  image-based  ai^xoacbes.  Rrst,  it  guarantees  that  an  obstacle  is  rented  to  tbe 
system  as  soon  as  it  is  detected  in  tbe  range  image  instead  of  waiting  for  tbe  entire  image  to  be  processed,  thus 
r^ucing  the  latency.  Second,  the  scanline  ai^noach  permits  a  sinq)le  scheduling  algorithm  for  syndironizing  range 
image  {Hocessing  with  other  tasks:  The  system  simply  executes  a  given  task,  e.g.,  sending  steering  evaluations  to  tbe 
planner,  after  processing  a  scanline  if  enough  time  has  elapsed  since  the  last  time  tbe  task  was  executed. 

Figure  11  shows  a  typical  example  of  scanline  processing  of  range  images.  Figure  11(a)  is  a  video  image  (rf  tbe  scene 
which  consists  of  a  corridor  limited  by  inqiassable  rocks  and  trees.  Hguie  11(b)  shows  a  64  x  256  range  image  of  the 
same  score.  Points  near  the  senstM'  are  da^  and  those  farther  away  are  light  Superinqrosed  on  the  range  image  are 
the  locatitms  of  two  scanlines.  Figure  11(c)  shows  an  overhead  view  of  the  pixels  from  each  of  the  scanlines  marked 
in  Hgure  11(b).  To  produce  this  display,  the  pixels  from  the  range  image  are  converted  to  Cartesian  coordinates  and 
projected  on  the  ground  plane.  The  impassable  regitms  are  displayed  as  bladk  dots  and  are  found  mostly  on  the  right 
side  of  the  vehicle,  corresponding  to  the  “wall”  visible  on  the  right  of  Hgure  11(a).  The  order  of  the  scanlines  in 
Figure  11(c)  refiects  the  orto  in  which  the  data  is  processed:  first  the  bottom  scanline  (closest  point)  and  then  the  top 
(farthest  point). 

Run-time  parameters  can  be  set  to  accommodate  the  maximum  anticipated  vehicle  speed.  For  a  maximum  speed  of  3 
m/sec,  only  the  upper  two-thirds  of  the  range  image  is  processed  at  a  rate  of  2(X)  msec/image.  A  list  of  inqiassable 
cells  is  sent  to  the  local  nuq)  manager  every  1(X)  msec.  In  this  configuration,  tbe  minimum  detection  range  for  a  30  cm 
object  is  10  meters,  although  the  system  can  detect  larger  objects  up  to  40  meters  from  tbe  sensor.  Tbe  maximum 
detection  range,  together  with  sensor  latency,  0.5  secAmage,  are  the  main  limitations  of  the  system. 

Figure  10:  Range  Image  Processing 


23.3  Local  Map  Mmagement 

The  local  is  an  array  of  cells  with  a  simider  structure  than  the  grid  used  in  the  range  data  processing  component 
Local  nuq)  cells  contain  only  abinaiy  flag  indicating  whether  the  cell  is  traversable;  if  it  is  not  tibe  cell  also  contains 
the  cooithnates  of  a  3-D  point  inside  the  obstacle.  The  positions  of  the  untraversable  cdls  in  die  local  are 
updated  at  regular  interv^,  currently  100  msec,  acctnp^g  to  vehicle  motion.  Hgure  12  shows  an  overbe^  view  of 
the  local  traversability  crmstructed  from  a  sequence  of  images  from  the  area  shown  in  Hgure  11(a).  In  this  exam¬ 

ple,  the  cells  are  40  cm  x  40  cm.  The  untraversable  cells  are  shown  as  small  squares;  the  vehicle  is  indicated  by  the 
rectangle  at  the  bottom  of  the  display. 

Figure  12:  A  Local  Map 
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2:3.4  StMring  Are  Evaluation  for  DAMN 

Tbe  data  in  the  local  is  used  for  generating  admissive  steering  commands.  We  give  here  only  a  brief  description 

of  the  approach  and  refer  the  reader  to  Keirsey  [13]  and  Langer  [17]  for  a  detailed  descrq)tion  of  the  planning  archi¬ 
tecture.  Each  steering  arc  is  evaluated  by  computing  the  distance  between  every  untravetsable  cell  in  the  local  map 
and  the  «c.  An  arc  receives  a  vote  of  -1  if  it  intersects  an  imtravers^le  cell;  if  not.  it  receives  a  vote  varying  mono- 
tonically  between  -1  and  +1  with  the  distance  to  the  nearest  untraversable  celL  After  the  vote  for  each  indivulual  arc 
is  craqaited,  the  entire  array  of  votes  is  sent  to  an  arbiter  module  [13]  which  generates  an  actual  steering  cmmumd 
that  is  sent  to  the  vehicle. 

Hgue  13  shows  an  example  of  arc  evaluation  for  the  local  mrq)  in  Figure  12.  The  distribution  of  votes  ran^  fiom  a 
miniiniim  turning  radius  of  -8  meters  to  a  maTimum  of  -t-S  meters.  The  curve  shows  the  maximum  votes  are  for 
moderate  right  turns  of  the  vehicle  and  ate  close  to -1  for  left  and  right  turns. 

Figure  13:  Distrfeution  of  Votes  for  the  Map  of  Figure  12 


2.3.5  Cell  Tranemieeion  to  D* 

The  system  described  so  far  is  one  of  our  convottional  mitonomous  driving  systems  [17].  In  order  to  use  SMARTY  in 
conjunction  with  the  D*  module,  we  added  a  direct  link  between  D*  aixl  Shl4RTY  (see  Figure  3),  because  D*  needs 
to  update  its  internal  map  based  on  the  infcHmation  extracted  from  the  range  images.  In  themy,  SMARTY  should  send 
lists  of  traversable  and  untraversable  cells  found  in  the  current  batch  of  range  pixels  to  D*  as  soon  as  new  data  is 
available.  In  practice,  however,  this  causes  D*  to  receive  data  faster  than  it  can  process  it,  due  to  the  overhead  in 
sending  and  receiving  messages. 

In  order  to  avoid  this  probtem,  the  lists  of  traversable  and  untraversable  cells  are  buffered  instead  of  being  sent  to  D* 
as  soon  as  they  are  computed.  In  addition,  the  position  of  the  vehicle  at  the  time  the  data  used  for  cmnputing  the 
current  cells  was  acquired  is  also  buffered.  The  vehicle  position  is  sent  to  D*  along  with  the  lists  of  cells.  The  position 
information  is  necessary  to  enable  D*  to  convert  the  vehicle-centered  cell  locations  to  cells  locatimis  in  its  internal 
global  rtuq).  After  a  new  line  of  range  data  is  processed,  SMARTY  flushes  the  buffer  if  either  of  two  conditions  is 
met: 

•  Enough  time  has  elrqrsed  since  the  last  message  to  D*.  The  time  interval  between  messages  is  S(X)  msec.  This 
value  is  set  empirically  for  die  hatdvirare  configuration  currently  used 

•  The  position  of  the  vehicle  at  the  time  the  most  recent  sensor  data  was  acquired  is  diffoent  fiom  the  position  of 
the  vehicle  at  the  time  the  data  in  the  buffer  was  acquired 

The  first  condition  ensures  that  messages  are  not  genoated  at  a  fiaquency  foo  high  for  D*  to  process  them.  The 
second  condition  is  necessary  because  the  messages  sent  to  D*  include  a  single  vehicle  position  so  that  they  cannot 
suxmnmodate  lists  of  cells  acquired  fitun  two  different  vehicle  positions.  This  message  protocol  provides  a  good 
comprranise  betwem  the  need  to  send  up-to-date  infomudion  to  D*  and  the  need  to  limit  the  numbo*  of  messages  to 
D*. 
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2.4  Ttw  DAMN  Systwn  for  Stooring  Arbitration 

Our  navigation  systems  includes  q)ecialized  nuxlules  such  as  otetacle  detection  (SMARTY)  and  path  planning  (D*). 
Eadi  of  the  modules  has  its  own  view  of  the  best  action  for  the  vehicle  to  take  at  every  step.  Wb  use  the  DAMN  archi¬ 
tecture  for  cnmhining  leoHnmendations  firom  different  modules  and  for  issuing  actual  driving  cmomands  to  the  vehi¬ 
cle  controller  [17][23][26]. 

The  DAMN  architecture  consists  of  an  arbiter  which  receives  steering  rectnnmendation  from  outside  modules  and 
combines  them  into  a  single  driving  conunand.  The  recmnmendations  are  in  the  form  of  votes  between  -1  and  +1  fw 
a  pre-defined  set  of  arcs.  A  vote  of  -1  means  that  the  external  module  has  determined  that  the  arc  should  not  be 
driven,  e.g.,  because  of  an  obstacle  blocking  the  path,  and  -t-l  means  that  the  path  is  highly  recmnmaided.  Fust,  the 
arbiter  ccHnbines  the  votes  fiom  all  the  external  modules  into  a  single  distribution  of  votes  for  the  list  of  arcs.  For  each 
arc,  DAMN  multiplies  the  votes  fnnn  each  module  by  its  module  weight  and  then  sums  these  weighted  votes  to 
produce  a  single,  composite  vote.  Second,  the  arbiter  chooses  the  arc  with  the  highest  ccMiqx)site  vote  and  sends  it  to 
the  vehicle  controller. 

In  practice,  each  module  is  a  separate  Unix  process  which  communicates  with  the  arbiter  through  the  TCX 
communication  system.  Becroise  ttey  may  have  very  differoit  cycle  times,  the  modules  operate  asynchronously.  The 
arbiter  sends  commands  to  the  controller  at  regular  intervals,  currently  100  msec,  and  updates  the  list  of  combined 
votes  whenever  new  votes  are  received  from  the  other  modules. 

Because  the  DAMN  arlriter  does  not  need  to  know  the  semantics  of  the  noodules  firom  which  it  annbines  votes,  it  is 
very  general  and  has  been  used  in  a  number  of  systems  with  different  configurations  of  modules  [30].  We  concentrate 
here  on  the  configuration  of  our  navigation  system  as  illustrated  in  Figure  3.  The  arbiter  receives  votes  fimn  two 
modutes,  D*  and  SMARTY.  The  global  navigator,  D*,  sends  votes  based  on  its  global  m^  and  the  goal  location.  The 
local  navigator,  SMARTY,  soids  votes  based  on  the  data  extracted  from  range  images.  The  former  generates  driving 
recommendations  based  on  global  path  constraints  while  the  latter  generates  reamunendations  based  on  a  detailed 
description  of  the  local  terrain.  Module  weights  of  0.9  and  0.1  were  used  for  SMARTY  and  D*  respectively.  This 
selection  has  the  effect  of  favoring  obstacle  avoidance  over  goal  acquisition,  since  it  is  more  important  to  miss 
obstacles  than  to  stay  on  course  to  the  goal. 

The  current  version  of  DAMN  allows  for  forward  motion,  but  it  does  not  evaluate  steering  directions  for  reverse 
driving.  Of  course,  this  is  not  a  problem  for  on-road  navigation  systems  or  for  systems  which  use  sufficient  a  priori 
knowledge  of  the  environmoiL  In  our  case,  however,  it  is  oitirely  possibte  that  the  only  way  for  the  vehicle  to  reach 
the  goal  is  to  drive  in  reverse  out  of  a  cul-de-sac.  This  capability  was  not  yet  added  to  DAMN  at  the  time  of  this 
writing  so  that  reverse  driving  had  to  be  simulated  by  mani^ly  driving  the  vehicle  out  of  occasional  cul-de-sacs.  We 
clearly  indicate  such  occurrences  in  the  examples  given  in  the  next  section. 


3.0  Experimental  Results 

Two  of  the  trial  runs  that  illustrate  different  aspects  of  the  system  are  examined  in  this  section.  The  system  was  run  at 
a  local  test  site  called  the  Slag  Heap,  located  about  ten  minutes  firmn  canqnis.  The  Slag  Heap  consists  of  a  large  (q)ai 
area  of  flat  terrain  bounded  by  a  bexm  on  one  side  and  a  large  plateau  on  the  other.  The  obstacles  consist  of  sparse 
mounds  of  slag  in  the  interior  of  the  flat  area  and  small  trees,  bushes,  rocks,  and  dd»is  around  its  edges.  An  access 
road  skirts  the  perimeter  of  the  area.  An  aerial  view  of  the  test  site  is  shown  in  Hgure  14.  The  dimensions  of  this  area 
are  ^rproximately  800  x  1000  meters. 
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Figiire  14:  Aarial  View  of  Slag  Heap  Test  Site 


3.1  Goal  Acquisition  with  Backtracking 

For  both  trials,  an  optimistic  map  was  used  (ie.,  all  cells  are  traversable).  S 1  and  G1  mark  the  start  and  goal  locations 
for  the  first  trial.  SI  was  located  in  the  opoi  area,  and  G1  was  located  oo  the  access  road  behind  the  large  plateau. 
These  points  were  chosen  so  that  badrtraddng  would  be  required  to  circumnavigate  the  plateau.  Dma  finmn  the  trial  at 
a  number  of  points  is  shown  in  Figure  IS  through  Figure  20.  Each  of  these  figures  consists  of  four  parts:  (a)  the  vehi¬ 
cle’s  ir^ectory  superinqxrsed  on  D*’s  mrqr,  (b)  the  ERIM  laser  rangefinder  image  at  a  selected  point  along  the  trsgec- 
tory;  (c)  SM/JtTY’s  lo^  obstacle  nuq)  at  this  same  point;  and  (d)  the  votes  fimn  SMARTY,  D*,  and  DAMN  at  this 
same  point 

In  Hgure  lS(a),  the  vehicle’s  tnyectory  is  depicted  by  the  black  curve.  The  smaU  rectangle  near  the  end  of  die  curve 
is  the  “selected  point”  finxn  whidi  die  data  for  subfigures  (b),  (c),  and  (d)  was  taken.  The  C-space  obstacles  are  shown 
in  daric  grey  and  the  high-cost  buffer  cells  in  light  grey.  In  Figure  lS(b),  grey  scales  encode  the  range  values  for  the 
laser  rangefinder,  with  darir  grey  values  near  the  sensor  and  li^t  grey  values  farther  away.  In  Figure  lS(c),  the  large 
rectangle  shows  the  vehicle’s  position,  and  the  small  squares  show  the  positions  of  untravnsable  cells  around  the 
vehicle.  In  Hgure  15(d),  the  steering  voles  for  each  mod^  are  shown,  ranging  fircnn  -1  to  -fl. 
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Figw«  15:  Driving  into  the  Cul-de-eac 


Figioe  IS  stkows  the  first  pottkm  of  the  trial  The  vehicle  began  pointing  away  firom  the  goal  so  it  immediately  turned 
around  and  headed  directly  toward  it  The  vehicle  encountered  a  large  obsunction,  initially  turned  to  the  left,  then 
looped  around  the  obstacle  to  the  right  and  drove  into  a  cul-de-sac.  At  the  selected  poini  SMARTY  voted  to  turn  right 
to  avoid  the  boundary  of  the-cul-de-sac,  and  D*  voted  in  a  similar  fashion  in  order  to  loop  bade  and  explore  the  other 
side  of  tte  cul-de-sac. 
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Figwc  16:  OiscovAring  th«  Cul-de-sac 


Hgme  16  shows  the  vdiicle  driving  side-to-side  discovering  the  bounds  of  the  cul-de-sac  with  its  sensoc  It  ajqiears 
that,  at  the  selected  point,  D*  preferred  to  venture  into  the  high-cost  buffer  rather  than  backtrack  out  of  the  cul-de-sac; 
it  was  overruled  by  SMARTY’s  votes  to  avoid  the  cluttoed  area  altogether.  Since  D*  considers  the  cost  to  the  goal 
(mly  from  the  ends  of  the  steering  arcs,  it  relies  on  SMARTY  to  steer  clear  of  obstacles  coincident  with,  or  near  to,  the 
arcs  themselves. 
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Figore  17:  Exiting  the  Cui-de-sac 


Vot*  SMARTY  VolM 


Once  tbe  vehicle  discovered  that  the  “route”  was  obstructed,  h  badctracked  out  of  the  cul-de-sac  as  shown  in  Figure 
17.  After  exiting,  the  vehicle  looped  back  in  an  attempt  to  drive  through  perceived  gtqis  in  the  surrounding  berm. 
Note  that  at  the  selected  point,  D*  voted  to  turn  right  and  head  back  toward  a  gap. 
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Figure  18:  Looping  Back  Toward  die  “Gap* 
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After  looping  back,  the  vehicle  closed  the  first  gap  with  data  bom  the  laser  rangefinder,  and  SMARTY  deflected  the 
vehicle  away  fiom  the  second  due  to  other  obstacles  in  the  vicinity  (see  Hgure  18). 
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Figure  19:  Driving  Throu^  and  Around  th«  Vegetated  Area 


In  Hguie  19,  the  vehicle  moved  into  an  area  densely  populated  with  small  trees  befme  driving  out  to  the  left  In  these 
types  of  areas,  the  vehicle  was  drivoi  predominantly  by  SMARTY,  since  obstacle  avoidance  takes  precedence  over 
the  goal  seeking  behavior.  After  emoging  from  the  area,  D*  guided  the  vehicle  around  the  perimeter  of  the  veg^ation 
and  into  another  small  cul-de-sac.  As  was  the  case  with  the  first  cul-de-sac,  the  limited  field  of  view  of  the  ERIM 
sensor  precluded  the  possibility  of  detecting  the  cul-de-sac  befcse  entry  and  avoiding  it  altogether. 

This  tinm  the  cul-de-sac  was  too  small  for  the  vehicle  to  esc^  without  driving  in  reverse.  Since  the  NAVLAB  n  is 
currently  unable  to  do  this  automatically,  the  vehicle  was  manually  driven  in  reverse  until  the  vehicle  exited  the  cul- 
de-sac.  Note  that,  at  the  selected  point,  SMARTY  detected  obstacles  in  all  directions  and  consequently  voted  against 
all  steering  arcs.  D*  favored  backing  out,  but  since  such  a  move  was  not  possible  autonomously,  it  voted  for  the  next 
best  options:  either  a  hard  left  or  hard  right  turn. 
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Figure  20:  Finding  the  Access  Road  that  Leads  to  the  Goal 
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After  backing  out,  tbe  vehicle  was  placed  under  autonomous  control  once  again  (see  Hgure  20).  It  drove  around  the 
perimeter  of  the  large  plateau,  found  an  entry  point  to  the  road  behind  the  plateau,  and  then  drove  along  the  access 
road  until  it  reached  the  goal. 

The  total  path  length  for  the  trial  was  1410  meters.  At  six  points  during  the  trial,  we  manually  intovened  to  steer  the 
vehicle.  Half  of  these  interventions  were  to  drive  the  vehicle  in  reverse,  and  the  other  half  were  steering  deflections  to 
avoid  water  or  mud  that  was  too  difficult  to  detect  with  a  lass’  rangefinder.  D*  sent  steering  recommsidations  to  the 
DAMN  arbiter  every  500  msec.  A  total  of  2952  sets  of  steering  recommendations  were  sent.  Since  each  set  consists 
of  51  steering  arcs,  a  total  of  150,552  global  paths  to  the  goal  were  computed  during  the  trial.  SMARTY  sent  6119 
messages  to  O’*  containing  a  total  of  1,851,381  terrain  cell  classifications.  Tbe  radius  of  C-sps%  expansion  was  2 
meters,  and  the  radius  of  each  high-cost  buffer  was  8  meters.  A  high-cost  cell  was  five  times  more  expensive  to 
traverse  than  a  traversable  ceU. 

The  number  of  cell  classifications  was  large  since  each  terrain  cell  is  likely  to  be  seen  more  than  once,  and  each 
occurrence  is  transmitted  to  D'*'.  It  is  also  important  to  note  that  the  classification  for  a  many  terrain  cells  changed 
repeatedly  from  one  sensor  reading  to  flie  next  This  effea  was  due  in  part  to  sensor  noise  and  in  part  to  the  fact  that 
the  classification  of  a  given  cell  inqrroves  in  accuracy  as  tbe  vehicle  draws  neater  and  tbe  sensor  gets  a  better  view. 
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Note  that  the  high-cost  buffer  was  essential  to  complete  the  boundaries  of  the  cul-de-sacs,  idateau,  and  roads.  Without 
it,  the  vehicle  would  need  to  loop  around  many  more  times  for  more  sensor  data  in  the  first  cul-de-sac  befcve  O'* 
became  convinced  the  route  was  obstructed. 

3,2  Using  Map  Data  from  Prior  IViais 

In  Hgure  14,  S2  and  G2  mark  the  start  and  goal  locations,  respectively,  for  the  second  uial.  The  start  was  chosen  to  be 
at  die  entrance  to  the  Slag  Hetqi  on  the  access  road,  and  the  goal  was  at  the  opposite  end  of  the  flat,  open  area.  Ibe 
objective  of  diis  trial  was  not  to  illustrate  a  difficult  path  to  die  goal;  instead,  it  was  to  illustrate  the  effects  of  multiide 
runs  over  the  same  area.  In  Figure  21,  the  vehicle  drove  down  the  access  road  and  across  the  opm  area  to  the  goal.  At 
the  goal  point,  the  vehicle  was  taken  out  autcmatic  control  and  driven  manually  to  another  portion  of  the  access 

road  (see  Hgure  22).  During  the  manually-driven  segment,  the  software  continued  to  run;  thus,  the  tasQ  was  updated 
with  obstacles  detected  by  the  sensors. 

Figure  21;  Driving  the  Initial  Path  to  the  Goal 
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Figure  22:  Manually  Driving  to  a  New  Start  Point 


F^ure  23:  Rnding  the  Goal  for  a  Second  Time 


The  vehicle  was  placed  undo*  automatic  control  again,  and  it  drove  down  a  segment  of  the  access  road  until  it  found 
an  entry  point  into  the  open  area.  It  then  proceeded  to  drive  across  the  open  area  to  the  goal,  avoiding  a  number  of 
obstacles  along  the  way  (see  Hgure  23). 


The  vehicle  was  driven  manually  once  again  to  a  new  start  point  (see  Figure  24)  and  placed  bade  under  automatic 
control.  It  then  drove  to  the  goal  for  a  third  time  (see  Figure  25).  Note  that,  in  its  third  path  to  the  goal,  the  vehicle 
used  Quq)  data  that  was  constructed  during  the  previous  traverses.  As  shown  in  the  figure,  the  vehicle  positioned  itself 
to  pass  between  two  obstacles  before  its  sensor  was  close  enough  to  spot  fiiem. 
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Figore  24:  Driving  Manually  to  a  Third  Start  Point 


The  length  of  the  vehicle’s  path  for  this  trial  was  1664  meters,  including  both  automatically  and  manually  driven 
segments.  D*  soit  a  total  of  3168  sets  of  steering  recommendations  to  DAMN;  thus,  a  total  of  161,568  global  paths 
were  computed.  SMARTY  sent  6581  messages  to  D*  widi  a  total  of  1,601,161  cell  classifications. 

4.0  Conclusions 

4.1  Summary 

Ihis  paper  describes  a  annifiete  navigation  system  for  goal  acquisition  in  unknown  enviromnents.  The  system  uses 
all  available  prim  nuq)  data  to  plan  a  route  to  the  goal  and  then  begins  to  follow  that  route,  using  its  laser  rangefinder 
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to  examine  tbe  terrain  in  fnmt  of  the  vehicle  for  obstacles.  If  a  discrepancy  is  discovered  between  the  smsca'  Hata  and 
the  map,  the  nu^  is  updated  and  a  new,  optimal  path  is  planned  to  the  goal.  Because  of  the  efficiency  of  D*,  this  new 
path  can  be  goierated  in  a  fractkm  of  a  second.  Both  the  global  navigator  (D*)  and  tbe  local  navigator  (SMARTY) 
send  steering  recommendations  to  the  steering  arinter  (DAMN).  Because  obstacle  avoidance  takes  precedence  over 
goal  acquisition,  the  votes  from  SMARTY  are  weighted  mrve  heavily  than  those  from  D*.  Thus,  in  areas  dense  with 
obstacles,  the  vehicle  is  driven  primarily  by  SMARTY,  while  in  open  areas,  it  is  primarily  driven  by  D**.  It  was  found 
that  the  high-cost  buffers  around  obstacles  were  essential  to  fill  in  gaps  between  obstacles  and  precliule  repeated  sens¬ 
ing  of  the  same  area.  It  was  also  discovered  that  the  two-dimensicnuil  qqvoximatirHi  of  tbe  vehicle’s  three-dimen¬ 
sional  configuration  space  did  not  seriously  impair  perfamance. 

To  our  knowledge,  this  system  is  the  first  to  demonstrate  efficient  goal  acquisition  and  obstacle  avoidance  on  a  teal 
robot  vehicle  operating  in  an  unstructured,  outdoor  environment 

4.2  Future  Work 

In  the  near-term,  a  number  of  inqnovements  will  be  made  to  mmimiTP.  unnecessary  processing  and  increase  overall 
system  speed.  An  enormous  number  of  terrain  cell  classifications  ate  transmitted  tom  SMARTY  to  D*.  Smne  of 
these  classifications  are  erroneous  due  to  noise  or  less-than-ideal  viewing  conditions.  Noise  filtering  and  verification 
of  tbe  classifications  across  sensor  images  would  increase  confidence  in  the  data  and  reduce  communications  traffic. 
Currently,  the  cell  classifications  received  by  D*  are  processed  sequentially  to  create  the  C-space  expansions  and 
high-cost  buffers.  This  rqrproach  is  highly  inefficient  givoi  typical  clustering  of  obstacles,  and  the  additional  compu¬ 
tational  burden  resulted  in  O'"  sending  less-than-optimal  steering  recommendations  in  cluttoed  areas  in  order  to  meet 
timing  deadlines.  Processing  the  obstacles  in  batch  mode  using  tbe  grassfire  transform  for  expansion  should  greatly 
reduce  this  overhead.  Furthermore,  we  will  develop  a  more  systematic  q>proacb  to  the  scheduling  of  the  interactions 
between  SMARTY  and  the  other  modules,  D*  and  arbiter.  Currently,  the  fiequency  at  which  SMARTY  srads  into- 
mation  to  the  other  modules  is  deteimined  enq)irically  for  a  typical  vehicle  speed.  We  will  develop  an  algorithm  that 
relates  all  the  system  parameters,  such  as  soisor  field  of  view  or  vehicle  speed,  to  the  communication  frequency.  This 
last  improvement  will  involve  first  moving  the  modules  to  a  real-time  operating  system  in  order  to  guarantee  repeat- 
able  p^ormance. 

In  tbe  far-term,  we  will  extend  D*  so  that  it  dynamically  allocates  new  map  stcnage  as  needed  rather  than  requiring 
tbe  nuq)  to  be  pre-allocated  to  a  fixed  size.  Furthermore,  we  will  add  tbe  capability  in  O'*  to  reason  about  vehicle 
maneuvers  (coupled  forward-backward  motim)  in  (sder  to  handle  very  clutte^  areas  that  require  such  conq;)licated 
motion.  We  will  include  a  mechanism  for  speed  control  in  SMARIT  in  addition  to  tbe  existing  mechanism  for 
steering  control.  Speed  control  involves  reasoning  about  tbe  distribution  of  map  cells  with  respect  to  the  vehicle  and 
issuing  recmnmendations  for  speed  settings  such  that  tbe  vehicle  slows  down  in  cluttered  environments.  The  speed 
recommendations  may  be  encoded  very  much  like  the  steering  recommendations:  a  set  of  votes  for  speed  values 
between  0  and  a  pre-set  maximum  speed.  Additionally,  we  will  improve  tbe  performance  of  SMARTY  in  terms  of 
speed  and  maximum  range  in  order  to  support  higher  vehicle  speed.  This  will  be  achieved  mostly  by  using  better 
sensors  such  as  single-line  laser  scanners  or  passive  stereo. 
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